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ABSTRACT 


This research involves the development of an adaptive control law for a space based 











two-lin.. robotic manipulator. Non-adaptive controllers are first obtained utilizing feedback 
linearization techniques. A direct adaptive controller is then developed through the linear 
parameterization of the system dynamics, and the implementation of a Kalman Filter 
based adaption law. The controllers are then simulated and compared for various levels 
of system parameter uncertainty. The adaptive controller is found to be superior to the 


non-adaptive controllers for high levels of system parameter uncertainty. The non-adaptive 






controller is found to compare favorably to the adaptive controller in some areas for low 






values of system parameter uncertainty. The non-adaptive controller is implemented 







experimentally, consistent with hardware constraints. Experimental results verify the need 








for adaptive control when system dynamics are present which have not been modelled. 
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I. INTRODUCTION 


A. BACKGROUND 

Robotic systems have been utilized to perform a wide 
variety of functions for many years. Their speed, precision 
and reliability make them well suited for applications ranging 
from routine manufacturing processes to interplanetary space 
exploration. As mankind seeks to reach out to other worlds, 
the robot will play a crucial role. 

Space based robotic systems are required to deal with some 
unique conditions which are not encountered by their 
terrestrial counterparts. The absence of a fixed base upon 
which to mount a robotic manipulator and the lack of 
Significant external friction to dampen the system require 
special consideration by a control systems engineer. An 
effective control system must not only reposition the 
manipulator, but counteract the forces imparted on the main 
body by such a maneuver. This problem is further complicated 
when the space based manipulator is called upon to handle an 
object with unknown mass and inertia properties. 

Robotic systems equations of motion can be developed 
through Lagrange’s equations and are highly non-linear. 
Standard linear control techniques are not well suited to this 


kind of model. One approach to the control problem is to use 


feedback linearization. This technique involves the 
development of a linearizing controller to cancel system non- 
linearities. Linear control techniques are then applied to the 
linearized system. 

When system parameter uncertainty is present, controller 
performance can be improved through the use of adaptive 
control in which the uncertain system parameters are estimated 
on-line. Research in adaptive control started in the early 
1950’s in connection with the design of autopilots foi high- 
performance aircraft. Practical applications in robotic 
control emerged in the 1980’s. Initial research relied on 
restrictive assumptions or approximations including 
linearization of robot dynamics, decoupling assumption for 
joint motors and slow variation of the inertia matrix [Ref. 1] 
[Ref. 2]){Ref. 3] (Ref. 4]. Later research resulted in the 
linear parameterization of robot dynamics allowing the 
adaptive controller to fully account for the non-linear, time- 
varying and coupled nature of robot dynamics (Ref. 5] (Ref. 6] 
[Ref. 7]. 

Controllers for the NPS Spacecraft Robotic Simulator (SRS) 
were first developed by Sorenson [Ref. 8] and later modified 
by Yale [Ref. 9]. Both developments assumed perfect knowledge 


of robotic system parameters. 


B. OVERVIEW AND OBJECTIVES 
The focus of this research is to develop an adaptive 


controller for the NPS SRS and to experimentally verify non- 


adaptive controller characteristics. The equations of motion 


and control laws are developed in Chapter II. Chapter III 
contains a comparison of controller performances for varying 
levels of parameter uncertainty. Non-adaptive controller 
experimental implementation is discussed in Chapter IV. 
Chapter V includes a summary of the conclusions as well as 


topics for future research. 





II. ANALYTICAL MODEL 


The analytical model used in this research represents a 
spacecraft with a two-link manipulator attached. The 
manipulator is maneuvered by motors at its shoulder and wrist 
while a momentum wheel holds the spacecraft centerbody steady 
in a desired orientation. Motion of the system is confined to 
two dimensions and the spacecraft centerbody is allowed to 
rotate but not translate. These restrictions facilitate 


comparison between analytical and experimental results. 


A. COORDINATE SYSTEMS 

An overall system schematic is shown in Figure 1. This 
diagram presents the system coordinate frames used to develop 
the equations of motion. The coordinate frames utilized are 
the same as those utilized by Yale [Ref. 9:pp 7-9] and are 
illustrated in Figure 1. The centerbody and manipulator links 
are assumed to be rigid bodies. Therefore, member 
lengths(L,,L,), distances to centers of mass(L.),L.,;,L..), and 


the location where the manipulator attaches to the 


centerbody(L,,@,) remain constant. An inertial axis system is 


located on the centerbody at the point of rotation. A body 
fixed coordinate frame uses the same origin as the inertial 


frame but rotates with the spacecraft centerbody. The x-axis 





of this frame points to the centerbody center of mass. The 
centerbody attitude, 8,, is the angle between the inertial x- 
axis and the spacecraft centerbody x-axis. Each manipulator 
has its own set of body axes. A coordinate frame attached to 
the manipulator shoulder aligns its x-axis along the 
loncitudinal axis of manipulator Link 1. The attitude of this 
link, ®,, is zero when the link lies on a ray extending from 
the inertial origin through the shoulder. The attitude of Link 
2 is defined by a coordinate frame attached to the elbow. The 
attitude of this link, 6,, is zero when the link is parallel 
with Link 1. A set of generalized coordinates, g, is chosen 
which describe the system include the centerbody attitude and 


joint angles for both manipulator links. 


a= [0,0,6,]7 (1) 


Centerbody 





Figure 1: System Schematic 
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B. EQUATIONS OF MOTION 
The equations of motion for this system are a special case 
of those developed by Yale [Yale93:pp. 9-24] and are derived 


using Lagrange’s equations for a dynamic system. 


kinetic energy. 

potential energy. 

the generalized coordinates vector. 
the generalized velocities vector. 


the vector of applied non-conservative forces. 


Using Lagrange’s equations, the equations of motion can be 


expressed in an alternate form. 


M(g) 6+G(g,g) +See 


where: 
* Mis a 3x3 inertia matrix. 


G is a 3xl vector which accounts for centripetal and 
Coriolis torques. 


* V is the potential energy of the system. 





Eq. (4) can be further simplified because the potential energy 


of the system is constant, and it becomes 


M(g) g+G(g, &) =O (4) 


The following sections develop expressions for the inertia 
matrix, M,Coriolis vector, G, and generalized force vector Q. 
1. Inertia Matrix, M 
The inertia matrix is found by calculating the system 


kinetic energy and expressing it in the form 
T=S97[M(g) lg (5) 


The total system kinetic energy, T, is the sum of the kinetic 


energy of all system components. 
T=T,+T,+T, (6) 


Where T,,T, and T, are the kinetic energies of the centerbody, 
Link 1 and Link 2 respectively. Kinetic energy of individual 


components can be found from 


2 


Zl eto, (ZZ) (7) 


where: 


« I; is the member moment of inertia about its center of 
mass. 


* @, is the member angular velocity. 
* m, 1s the member mass. 


* r is the inertial velocity of the member center of mass. 


Kinetic energies for individual members were determined by 

Yale (Yale93: pp 12-14] and are contained in Appendix A. 
After substituting the expressions for kinetic energy 

obtained from Eqs. (6) and (7) into Eq.(4), the inertia matrix, 


M, is determined and is of the form 


Mi, Mey Mis 
MM. Mea Mas (8) 
M;, M,, M,, 


Expressions for the individual elements in the inertia matrix 


are given by 


M,,=I,+m1z, (9) 
M,,=M,2=M,,+m,1,1,,c0s8, (10) 
M,,=M,,=M,,+m,1,1,,cos (8, +8.) (11) 
M,,=M,,+1,+m,1,1,,c0s0,+m,13,+m,1? (12) 
M,.=M,,=M,2+1, (m,1,,+m,1,) cos8, +m,1,1,,cos (6, +6,) (13) 


M, ,=My2+Ip+My1éo+ (m,+m,) 1f+21, (m,1,,+m,1,) cos8,+2m,1,1,,cos (8, +8,; 
(14) 
2. Centripetal and Coriolis vector, G 
The Coriolis vector, G, contains all of the 


centripetal and Coriolis terms and may be found using 


at 
Gig, a) 97C) oo 
fogleatare; 
where the elements C"’ are defined by the Christoffel symbol 


{Yale93: p 17) 


(1) 1( OMs; | OM, | OMsy 16 
Cy 3( Sar bas Oa (16) 


The G vector for the system is of the form 


G=[G,G,G,)7 (17) 


Expressions for individual elements of the G vector are given 


by 


G,=-1, (67+26,6, ) (m,1,,+m,1,) sin6,-m,1,1,,8, (26,+26, +8.) sind, 
-m,1,1,, (26, (6, +6,) + (6, +6,) 7) sin (6, +6,) 
G,= 1,03 (m,1,,+m,1,) sin6, -m,1,1,,6, (26,+26, +6,) sin6, ai 
+m,1,1,,63sin (6, +8,) 
G,=m,1,1,,(6,+6,) ?sin8,+m,1,1,,63sin (6, +6.) (20) 
3. Generalized Forces, Q 
It is beneficial to express the vector of generalized 
forces, Q, in terms of torque vector U, representing torques 


applied by individual actuators. This is accomplished using 


the principle of virtual work. The total virtual work is the 


10 


sum of the torques applied to each system component multiplied 


by each component’s virtual angular displacement. 
3 
oy). bn, (21) 
el 


The local torque vector is simply a 3 by 1 vector consisting 
of torques applied by the centerbody, shoulder, and elbow 


actuators respectively. 


U-[U,U,0,)7 (22) 


Because the angles describing the system are expressed in 
local coordinates each actuator creates a virtual displacement 


only for its associated component. 
bw,=U,88, (24) 
This yields the relation, 
Q=0 (25) 
4. Equations of Motion: Expressed in Local Coordinates 
Substituting Eqs.(9)-(14),(18)-(20) and (25) into 
Eq.(4) produces the system equations of motion expressed as a 
function of local coordinates 


M(8)6+G(6,6) =U (26) 


Where, 


11 








8=(6,6,6,)" (27) 
Eq.(26) is the equation of motion upon which system control 


laws are developed. 


C. REFERENCE MANEUVER 

Controller torques will be designed to cause the system to 
follow a desired reference trajectory. Given a three degree of 
freedom system, one needs only to specify the trajectory to be 
followed by three of the system’s generalized coordinates. The 
remaining set of generalized coordinate trajectories can be 
found via geometric reasoning. 

1. Selection of Reference Trajectory 

The three coordinates chosen to be specified by the 
reference trajectory are the actuator tip x and y coordinates, 
R, and R,, and the centerbody angle 6. The generalized 
coordinates used in the system equations of motion, Eq. ‘26), 
are 6), 0, and 6,. R, and R, can be expressed in terms of 0, 9, 
and @,. 

The choices between reference trajectories which move 
the system from a given initial condition to a desired final 
condition are infinite. To help ensure that the spacecraft 
structure does not experience any unnecessary jerks or 
excitation of flexible structures, Sorenson utilized a fifth 
order polynomial with zero velocity and acceleration at 


initial and final conditions [Sore: pp 25-29]. 
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£(t) =6t9-1504+1007 (28) 
Where the normalized time, T, is defined as 


t-t, 
Cr- ly 





(29) 


Given the desire to hold the centerbody attitude constant 
during a given manipulator maneuver, the centerbody angle, 
angular velocity and angular acceleration reference maneuvers 


are simply 

6,=0 

6,=0 (30) 
§,=0 


The manipulator tip position velocity and acceleration (R, R, 


) are found via 


R(t) =R( C9) +£(t) (R( Cy) -B( CQ) ] 


| 


Ce- Cy 


ate) -2¢2)| (31) 


R(t) Bn ATE =F | 


(-C)? 

Where R(t) is the position vector originating at the 

centerbody point of rotation and ending at the tip of Link 2. 
2. Coordinate Transformation 


The system control laws to be developed will require 


angular position, velocity and acceleration of all elements of 








the system generalized coordinate vector g. These can be found 
geometrically. 

Generalized coordinates 6,,R, and R, along with their 
respective velocities and accelerations have been derived in 


the previous section. 


(a 


61) 


Figure 2: Manipulator Joint Angle Derivation Schematic 


The position of the shoulder (S,, S,) and the magnitude of the 
vector between the shoulder and the tip of Link 2 (SR) can 


then be expressed as 
S,=1,cos (8,+8,) 
S,=1,sin (6)+6,) (32) 
SR= (R,-S,) + (R,-S,) 

Angles formed by the triangle formed by SR and the two 


manipulator links are found using the law of cosines. 





Bratar 2) (33) 
x x 

: 1f+SR?-13 (34) 
prracod 21,5R 

_ If +17-SR? (35) 


The local angles are then found to be 
6,=B8,-B- (6,+8,) (36) 
6,=180°-B, (37) 
Manipulator joint velocities and accelerations are found by 


expressing the manipulator end point coordinates in terms of 


system generalized coordinates. 
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R,=1,cos (0,+8,) +1,cos (6,+6,+6,) +1,cos (6,+6,+6,+8,) (38) 


R,=1,sin (6,+6,) +1,sin (6,+6,+68,) +1,5in (6,+6, +6, +6,) (39) 


Upon differentiation these equations can be expressed in the 


form 


RB) 16 
Ba ie 


Where H is the Hamiltonian matrix expressed in the form 


H= A, = (41) 
Hz, He 





Individual elements of H are found to be 


H,,=-1,8in (6,+6, +6, +6,) -1,sin (6,+6,+6,) 
H,,=-1,s8in (6,+8,+6, +6,) 

H,,=1,cos (6,+6,+6, +6,) +1,cos (6,+6,+6,) 
H,,=1,c0s (6,+6,+6, +6.) 


(42) 


On the basis of Eq.(40)joint velocities are then found as 
oe R, (43) 
6, R, 


together with the joint accelerations 


16 





&)_., [6]... (0 
ele) i 


Rearranging Eq. (44) we obtain 


betel] a 
2 Y. 2 
3. Reference Torques 

Given the reference values for the system generalized 
coordinates and their velocities and accelerations, a 
reference torque, U,, can be derived which would produce 
perfect tracking in the case of no external disturbances or 
modelling errors. The derived reference torque alone would 
represent an open loop type controller. 

The system reference torque is derived by evaluating 
the system equations of motion at the reference values of the 
system generalized coordinates and their higher order 


derivatives as presented in Eq. (26). 


D. NON-ADAPTIVE CONTROL LAW DESIGN 

In this section two non-adaptive control laws are 
developed for the space based robotic manipulator. The first 
uses feedback linearization to cancel system non-linearities 
in conjunction with a PD type controller. The second is a 


modification to the first in which the non-linear portion of 
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the controller utilizes generalized coordinate reference 
values instead of state feedback. 
1. Linearizing Controller 
a. Controller Design 

The control law presented utilizes feedback 
linearization to cancel out non-linearities which occur in the 
system inertia matrix, M, and Coriolis vector, G. A PD 
controller is then applied to the linearized system. The 


control law can be expressed in the following form 
U=U,-8u (46) 


The linearizing term, U,, serves to cancel system non- 
linearities by predicting the current values of the system 


inertia matrix and Coriolis vector. 


,=M(8) 8 +G(8,8) (47) 


The PD control term, $8U, corrects for tracking errors 


encountered by providing state feedback to the system. 


bu=M(8) [-X, (8-8) -x,(8-8,)] (48) 


A controller block diagram is presented in Figure 3. 
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Linearizing Controller Block Diagram 


Figure 3: 
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b. Control Law Stability 
Control law stability is determined by considering 
the behavior of the system trajectory error, e. The trajectory 


error and its first and second derivatives are defined as 


e=8-9 (49) 


2-0-6. (50) 


2-0-0. 


Substituting Eqs, (49)-(51) into Eq. (46) yields 


U=G(8,8) +M(8) (8 -K,é-K,e] (52) 


which in turn can be combined with Eqs (50) and (25) to obtain 


M(Q) (8+K,@+K,e) =0 


Because the system inertia matrix is positive definite and 


thus invertible, for any positive definite K, and k, 
lim,_. e( t) =Q 


2. Reference Controller 
The reference controller presented is merely a slight 
modification to the linearizing controller presented 
previously. Rather than picking an inertia matrix and Coriolis 
vector to cancel out system non-linearities, M and G are 


calculated based on where the system should be as determined 





by a desired reference trajectory. The form of the controller 


is similar to that of the linearizing controller 
Q-0,-8u (55) 


The PD control term, 6U, is identical to that presented in Eq. 
(48). The linearizing term, U,, serves to cancel system non- 
linearities by predicting the reference torques required to 
produce the desired reference trajectory when perfect tracking 


is assumed. 


u,=M(8,)8,+6(8,,0,) 


A controller block diagram is presented in Figure 4. 


E. ADAPTIVE CONTROL LAW DESIGN 

Robotic manipulators are designed in order to grasp or 
manipulate an object. Often the mass and inertia properties of 
the object are not known beforehand. This in addition to 
modelling errors leads to uncertainty in system parameters and 
degraded control law performance. Adaptive control utilizes 
system input and output data to update the system parameters 
and thereby adjust to changes in system parameters. 

1. Control Law Design 

The adaptive controller developed is merely a 

modification to the non-adaptive reference controller 
presented in Eq.(55). The only difference between this 


controller and the non-adaptive version is a dependance on an 


a1 





ba 
[9 ) 
3 
a 
oO 
O 


Linear 
Controller 





Figure 4: Reference Controller Block Diagram 
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estimate of the system parameter vector A. In the non-adaptive 
case the system inertia matrix, M, and G vector are functions 


only of the generalized coordinates. In the adaptive case, M 


and G are functions of both generalized coordinates and the 


system parameter vector estimate A. 


M(9, A) 
G(8,8,8) 


(57) 


The meaning of A is developed in the following section. A 
controller block diagram is presented in Figure 4. 
2. System Parameterization 
The system parameter vector A is determined by 


expressing Eq.(25) in an alternate form 


©7(6,6,8) A=u (58) 


where ® is a function of the system generalized coordinates 
and A is a function of the system parameters. Equating 


Eqs.(26) and (58), A is found to be 


A(1) =I,+m1é, 

A(2) =m1,1,, 

A(3) =m,1,1,, 

A(4) =2,+m,12,+m,1? 
A(5) =1,(m,1,,+m,1,) 


A(6) =Iy+Mg1Zo+ (m,+m,) 19 


The matrix of generalized coordinate ® is of the form 





Mey uondepy 





Figure 5: Adaptive Controller Block Diagram 
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Expressions for the individual elements of ® are given by 


®,,=6,+6, +8, 
®,,=%,, 


®,,=%,, 


®,,= (26,+26, +6,) cos@,-6, (26,+26, +8,) sind, 


®.,,=9%,, 

®,,= (6,+6,) cos6,+ (6, +6,) 2sin8, 

®,,= (20,+6, +8.) cos (6, +6,) - (26, (6, +6.) + (6, +6,) 2) sin (6, +6.) 
®,,=8,cos (6,+6,) +6%sin (6, +6,) 


,,79,, 


(61) 


(62) 


(63) 


(64) 


(65) 


(66) 


(67) 


(68) 


(69) 





©,,=6,+6, 


© .2=O4, 


®,,= (26,+6,) cos, - (67+26,6,) sin, 


®,,=6,cos@, +87sin®, 
©,,=6, 


Adaption Law 
The system parameter vector, A, is updated via a 
recursive Kalman Filter. The standard Kalman Filter state 


space equations can be expressed in the form 


&(t+1) =0x(t) +Aw(t) (75) 
v(t) =Cx(t) +y(t) 


Assuming a constant system parameter vector A for a given 


maneuver, the system can be expressed in state space form as 


A(t+1) =A(t) +w(¢) 
U(t) =®7(t) A(t) +¥(e) 


(76) 


Equating Eqs.(75) and (76) yields the following relations. 





Applying standard Kalman Filter equations to Eq.(76) yields 


the following set of recursive equations 


A(t+1) =A(¢) +K(t) (u(t) -®7(¢) a(t)) 
K(t) =P(t) ®(t) [A+®7(¢) P(t) ®(t)] 7 
P(t+1) =P(t) -K(t) O7(cv, P(t) +9 


where 
* K(t) is the Kalman Filter gain. 
* A is the noise covariance matrix, E[e(t) e*(t)]). 


* P is the parameter error covariance matrix, E{ (A(t) - 
Becuea) (A(t) oRecean) . 


* © is the plant noise covariance matrix, Elv(t)v7(t)]. 
Because the noise covariance matrices, A and Q, are not known, 
the parameter error covariance matrix, P, and the plant noise 


covariance matrix, Q, are redefined 


(79) 


Combining Eqs.(23) and (24) yields the recursive equations 





&(t+1) =4(t) +K(t) [u(c) -®7(t) a(t) ) 
K(t) =P(t) ®(t) [Z+@7(t) P(t) ®(t))7 (80) 
P(t+1) =P(t) -K(t) ®7(t) P(t) +9 
Eq. (80) provides the recursive 


equations necessary to update 
the system parameter vector, A. 
In the next chapter the three controllers are 


implemented for various levels of parameter uncertainty and 


their performances compared. 
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III. SIMULATION RESULTS 


The computer simulations presented in this chapter were 
obtained using the MATLAB subroutines listed in Appendix B. 
Simulations are presented for various levels of system 
parameter uncertainty ranging from 0% to 500%. System 
parameter values presented in Table 1 correspond to actual 
values of the Spacecraft Robotics Simulator and are utilized 
to simulate system dynamics. Parameter values used by the 
controller contain a random error up to a specified percentage 
of the actual parameter value. 

Equations of motion and computer code are verified by 
examining the change in angular momentum of the system for 
each simulation. For a given maneuver the rate of change in 
angular momentum will equal the sum of external torques on the 
system. The only external torque experienced by the Spacecraft 
Robotic Simulator is produced by the centerbody momentum 


wheel. Thus, for each simulation the relation 


should be satisfied. Where H is the rate of change in angular 
Momentum and Uvee; 18 the torque produced by the centerbody 
momentum wheel. The right hand side of Eq(77) was found to be 


< 10°** Nm/s* for all simulations. 





A. SIMULATION TEST CASES 
Simulation results are presented for five cases. The first 
case trains the adaptive control to recognize centerbody 
characteristics. Cases 2-6 examine the effects of parameter 
uncertainty on a desired manipulator maneuver. During the 
maneuver, the manipulator tip is repositioned from an initial 
to a final point along a straight line between the two points 
as shown in Figure 6. The angular position of the centerbody 
is held constant. 
1. Case 1: Adaptive Controller Training Maneuver 
Adaptive parameter A(6) depends only on centerbody 
characteristics. In order to update this parameter, a 
centerbody maneuver is required. Cases 2-6 attempt to hold the 
centerbody fixed and produces a small centerbody angular 
position, velocity and acceleration. A separate case, in which 
the centerbody is maneuvered, is required to adaptively update 
centerbody characteristics. During this maneuver, the 
manipulators are maneuvered in accordance with the reference 
maneuver pictured in Figure 6 while the centerbody is 
maneuvered as shown in Figure 7. Once the centerbody 
parameter,A(6), iS updated, it is assumed fixed and no error 


1s induced into this parameter in Cases 2-6. 
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TABLE 1: SYSTEM PARAMETER VALUES 





Parameter Name Parameter Variable Parameter Value 









Centerbody Radius 







0.530 m 
0.533 m 


Arm 1 Length 


Arm 2 Length 


; Centerbody CM 0.104 m 
| 0.403 m 
0.314 m 
65.96 kg 
2.34 kg 
2.86 kg 


5.74 kg-m? 


farm 1 CM 


f Arm 2 CM 


Centerbody Mass 


| Arm 1 Mass 


j Arm 2 Mass 


Centerbody Inertia 


land 
io 


i: ° io ° 


l 


Arm 1 Inertia 0.081 kg-m? 
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Figure 6: Reference Maneuver Time Lapse Stick Figure 
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Figure 7: Centerbody Parameter Training Maneuver 
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2. Cases 2-6: 0-500% Parameter Uncertainty 
Controller performance is presented for various levels 
of parameter uncertainty ranging from 0-500%. Controller 


errors and adaptive parameter updates are presented in Figures 


8-23. Centerbody control torque characteristics are presented 


in Table 2. 


B. COMPARISON OF CONTROLLERS 
1. Adaptive Controller vs Non-adaptive Controller 
The adaptive controller is clearly superior to the 
non-adaptive controllers for large values of parameter 
uncertainty (>50% parameter uncertainty). For small values of 
parameter uncertainty, the linearizing controller is superior 
to the adaptive controller in all but centerbody control. 
2. Linearizing Controller vs Reference Controller 
The linearizing controller is superior to the 
reference controller for low to moderate values of parameter 
uncertainty (<150% uncertainty). The reference controller 
exhibits superior performance over the linearizing controller 


for the case of 150% parameter uncertainty. 





TABLE 2: CENTERBODY CONTROL TORQUE CHARACTERISTICS 























Controller Unnee: Max Uvnee: Min Jabs (Usnee) ) 
7 Type/ 
f parameter 
f uncertaint 


0.2468 





Linearizing 0.3735 -0.3987 
(0%) 
| Reference 0.3735 ~0.3987 
(0%) 


0.2468 


0.2464 


# Linearizing 0.3723 ~0.3987 
(50%) 

i Reference (50%) | 0.3738 ~0.3980 
J Adaptive (50%) | 0.3735 ~0.3987 


Linearizing 0.3741 ~0.3963 
| (100%) 
1 Reference 0.3742 ~0.4000 
| (100%) 
J Adaptive (1008) | 0.3735 ~0.3987 


f Linearizing 0.3736 ~0.3958 
(150%) 


I Reference 0.3744 -0.3930 
A (1508) 


| Adaptive (150%) | 0.3734 -0.3987 
1.6784 -2.1698 


0.2472 
0.2468 
0.2489 


0.2490 


> 
Q 
o 
O 
a 
w 
< 
© 
a 
oO 
i K< 


0.2468 
0.2496 


0.2491 
0.2470 


0.2659 
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Figure 8: Linearizing Controller Error 
(0% parameter uncertainty) 
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Figure 9: Reference Controller Error 
(0% parameter uncertainty) 
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Figure 10: Linearizing Controller Error 
(50% parameter uncertainty) 
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Figure 11: Reference Controller Error 
(50% parameter uncertainty) 
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Figure 12: Adaptive Controller Error 
(50% parameter uncertainty) 
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Figure 13: Adaptive Parameter Updates 
(50% parameter uncertainty) 
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Figure 14: Linearizing Controller Error 
(100% parameter uncertainty) 
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Figure 15: Reference Controller Error 
(100% parameter uncertainty) 


38 


310~*% Tneted Error ve Time Tnheto! & Thete? Error ve Time 


10 
Time (sec) Time (sec) 


x Error vs Time 


Time (sec) Time (sec) 





Figure 16: Adaptive Controller Error 
(100% parameter uncertainty) 
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Figure 17: Adaptive Parameter Updates 
(100% parameter uncertainty) 
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Figure 18: Linearizing Controller Error 
(150% parameter uncertainty) 
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Figure 19: Reference Controller Error 
(150% parameter uncertainty) 
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Figure 20: Adaptive Controller Error 
(150% parameter uncertainty) 
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Figure 21: Adaptive Parameter Updates 
(150% parameter uncertainty) 
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Figure 22: Adaptive Controller Error 
(500% parameter uncertainty) 
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Figure 23: Adaptive Parameter Updates 
(500% parameter uncertainty) 





IV. EXPERIMENTAL WORK 


The spacecraft Robotics Simulator (SRS) was utilized for 
the experimental portion of the thesis. The SRS is a 
derivative of the Flexible Spacecraft Simulator (FSS) 
initially developed by Watkins [Ref. 10] and later modified by 
Hailey [Ref. 11]. Sorenson [Ref. 8] began the work to convert 
the FSS into the SRS. The robotic manipulator utilized was 


developed by Yale [Ref. 9]. 


A. SETUP 

The SRS permits experimental investigation of two- 
dimensional robotics motion and rotational spacecraft dynamics 
and is illustrated in Figures 24 and 25. The simulation 
hardware is floated on an eight foot by six foot granite table 
by means of a thin layer of air supplied by an external 
source. The table is polished to within 0.001 inch peak to 
valley and leveled to prevent gravitational accelerations from 
impacting the motion across its surface. The following 
sections describe the simulated spacecraft with its associated 
sensors and actuators and the controller which together form 
the SRS. The spacecraft components are the centerbody and two- 


link robotic manipulator. 
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Figure 24: Spacecraft Robotic Simulator 





Centerbody 





Figure 25: SRS Top View 
1. Centerbody 

The centerbody is a 30 inch diameter, 7/8 inch thick 
aluminum disk. It carries a position sensor, rate sensor, 
momentum wheel, thrusters, and an air tank to power the 
thrusters. The centerbody also serves as the mounting platform 
for the manipulators and is floated by a control air bearing 
and three air pads located at 120 degree intervals near the 
outer edge. The air pads are each capable of floating 60 
pounds when the air pressure supplied to the pads is 80 psi. 
The centerbody is allowed to float freely on the granite 


table. 
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The centerbody angular rate is measured by a rate 
transducer manufactured by Humphrey, Inc., having a range of 
+100 deg/sec and a minimum threshold of 0.01 deg/sec. The 
centerbody’s translation is not measured and is neglected for 
the experiment. 

The centerbody’s angular position is controlled by a 
momentum wheel and is powered by a model JRI6M4CH/F9T servo 
motor manufactured by PMI Industries whose characteristics are 
summarized in Table 3. Although the centerkody also carries 


two thrusters, they are not used in this research. 


TABLE 3: MOMENTUM WHEEL MOTOR CHARACTERISTICS 


| Manufacturer 
I Model JR16M4CH/F9T 


eam 6. ee 
[ee EE 
Ketaeiee ae od 


2. Manipulators 


PMI Industries 
















“I a ~ 
a wn ~s 
© 


The two-link manipulator has three joints. The 
shoulder joint is supported by the centerbody while the elbow 


and wrist joints are supported by two air pads apiece. The 
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links between the joints are stiff laterally but permit some 
flexibility vertically. This feature increases the tolerances 
on the air pad height adjustment. Joint angles for the 
shoulder and elbow are sensed by encoders purchased with the 
joint actuators. The encoder resolution is 0.005 degrees. 
Manipulator actuators are harmonic drive dc servo actuators 
manufactured by HD Systems, Inc. The shoulder actuator is 
model RFS-25-6018-E036AL while the elbow and wrist actuators 
are model RFS-20-6012-E036AL. Specifications for joint 
actuators are contained in Table 4. The wrist joint actuator 
and sensor is not utilized in this experiment. Manipulator 
schematics are contained in Figure 26. 

The joint actuators are all driven by Kepco power 
Supplies. These bipolar, programmable, linear amplifiers can 
be controlled manually from the front panel or controlled 
remotely with a +10 volt signal. In this application, The 
power supplies are operated in the current control mode with 
the voltage and current limits manually set consistent with 
the values in Table 4. The specific power supply models and 


their characteristics are summarized in Table 5. 
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TABLE 4: MANIPULATOR ACTUATOR CHARACTERISTICS 


epee ite conan = ie ee 
fp rated current (amps) | zg | 
[ated vottage woitsy | 5 | ts 
| rated Torque (in-tyy faa | tc 
a ee a eee 
es ere 
merrree 













Height (in) 
Weight (lb) 


Actuator/Encoder 


Air Pad 
Figure 26: Manipulator Top and Side Views 
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3. Controller 

The AC-100 programmable controller manufactured by 
Integrated Systems, Inc. controls the SRS. The AC-100 includes 
an Intel 80386 Application Processor, an Intel 80386 Multibus 
II Input/Output Processor, an Intel 80386 Communication 
Processor, an Intel 80387 Coprocessor, a Weitek 3167 
Coprocessor, an Analog-To-Digital and Digital-To-Analog Data 
Translation DT2402 Input/Output Board, two INX-04 Encoder and 


Digital-To-Analog Servo Boards, and an Ethernet Interface 


TABLE 5: POWER SUPPLY CHARACTERISTICS 


fvode 80 72-60 BOP_72-3M 
Actuator Controlled Right Shoulder Right Elbow 


Closed Loop Gain 0.6 (amp/volt) .3 (amp/volt) 


Module. The AC-100 also includes software installed on a VAX- 
3100 Series Model 30 workstation. The software permits design 
of a controller in block diagram graphical form and conversion 
of the diagram to C language programming code. The user is 
also able to design an interactive animation window to operate 
the controller. The AC-100 receives input signals from the 
sensors and the graphical user interface. AC-100 output 
signals go to the power supplies driving the actuators or to 


the graphical user interface for display. 





4. System Integration 
Several problems were encountered while attempting to 


implement the non-adaptive reference controller 


experimentally. As a result several modifications to the 


experiment were made. 
a. Actuator Dead Zonea 

The first problem was caused by the large dead 
zones present in the harmonic drive motors. Both the shoulder 
and wrist motors were designed to be operated in a high torque 
environment. The SRS components are relatively small and offer 
little resistance to motion. This resulted in reference 
torques for a reasonable maneuver which were entirely within 
the dead zone of system actuators. This caused system tracking 
errors to build up until the PD control term produced torques 
larger than the actuator dead zones. 

b. Centerbody Resistance 

A second problem involved a noticeable resistance 
to rotation by the centerbody. This is due in part to the 
inability of the central air bearing to function except under 
very low lateral loading. This lateral loading was eliminated 
by allowing the centerbody to float freely and ignoring 
translation of the centerbody. This decreased some centerbody 
resistance to rotation but some resistance was still detected 
due to the effects of external wiring necessary for centerbody 


components. 





c. Momentum Wheel Control 

A third problem involved the ability to safely 
control the centerbody reaction wheel. The AC-100 control 
software was found to be subject to periodic freeze ups in 
which controller outputs could not be reliably predicted. The 
momentum wheel could therefore not be safely utilized. The 
experimental modification was to perform runs in which the 
centerbody was held fixed to simulate perfect reaction wheel 


performance. 


B. RESULTS 
Results are presented in Figures 27-40 for four control 
cases. 
1. Case 1: High Gain, Free Centerbody 
This case utilizes a high gain controller to control 
a system in which centerbody motion is not constrained. The 
controller gains utilized are presented in Table 6. 
2. Case 2: Low Gain Controller, Free Centerbody 
In this case, a low gain controller is utilized toa 
control a system in which the centerbody motion is not 
constrained. The controller gains utilized for this case are 


presented in Table 6. 
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3. Case 3: High Gain Controller, Fixed Centerbody 
The controller utilized in this case is identical to 
that of Case 1 but the centerbody is now held in a fixed 
position. 
4. Case 4: Low Gain Controller, Fixed Centerbody 
This controller is identical to Case 2 but is used to 
control a system in which the centerbody position is held 


fixed as in Case 3. 


C. COMPARISON OF CONTROLLERS 
1. High vs Low Gain 
The high gain controllers yielded lower steady state 
errors than the low gain controllers at the cost of larger 
oscillations during the maneuver. 
2. Free vs Fixed Centerbody 
When the centerbody is allowed to float freely, 
Significant errors in centerbody attitude and manipulator tip 


position are seen. 
TABLE 6: CONTROLLER GAINS 


K, (low gain 1000 | 
controller) 
(high gain 1000 2000 
reece 
(low gain 10 25 
Me pee ide \ 
K, (high gain 20 50 | 
controller) 
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V. SUMMARY AND CONCLUSIONS 


A. SUMMARY 

Three controllers were successfully developed for a spaced 
based two-link robotic manipulator. Two non-adaptive 
controllers (a linearizing controller and a reference 
controller) were first developed using feedback linearization 
techniques. An adaptive controller was then developed through 
the linear parameterization of system dynamics and the use of 
a recursive Kalman Filter based adaption law. Controllers were 


then compared for system parameter errors up to +500%. 


Centerbody pointing accuracy was improved by utilizing 


adaptive control while centerbody control torque was effected 
very little. For high values of parameter uncertainty, 
manipulator tracking errors were smaller when using the 
adaptive controller. For low values of parameter uncertainty, 
the linearizing non-adaptive controller outperformed the 
adaptive controller in some areas. 

Implementation of the non-adaptive reference controller 
experimentally demonstrated the effects of un-modelled system 
dynamics. The oscillations and steady state errors encountered 
only reinforced the value of adaptive control in real world 
applications. High PD control gains produced = larger 


oscillations but smaller steady state errors than low gains. 





Manipulator maneuvers produced significant disturbances on 
centerbody attitude when no control was applied to hold the 


centerbody steady. 


B. RECOMMENDATIONS FOR FURTHER STUDY 

Feedback linearization represents only one way to attack 
a non-linear control problem. Other approaches include neural 
networks and sliding mode controllers. 

The adaptive controller developed may be too cumbersome to 
implement in real time. In order to decrease computation time 
the system can be re-parameterized in terms of only payload 
characteristics and a more efficient adaption law developed. 

Experimental implementation inaccuracies resulted from un- 
modelled actuator dead zones. System equations of motion and 
reference torques can be reformulated to include this effect. 


The use of fuzzy logic is also worth investigating. 
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APPENDIX A: MEMBER KINETIC ENERGIES 
Kinetic energy of individual components is found using 


Eq. (7) 
Ty 5 Iyo}+ Sm, (£2) (7) 
The centerbody angular rate and center of mass position vector 
are given by 
@,=6, (79) 


£, =Log% (80) 
Differentiating Eq.(80) results in the velocity of the 
centerbody center of mass 
Z, =Leg@oVo (81) 


Substituting Eq.(79) and (81) back into Eq.(7) and collecting 


on the angular rate term leads to 
Ts (I,+m,L2o) 62 (82) 


Similar developments are used for each of the remaining 
pieces in the system. For the manipulator link between the 
shoulder and elbow (Link 1), the angular velocity is a 
combination of centerbody rotation and rotation of the link 


with respect to the centerbody. 
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w, =6, +6, (83) 


The position vector to the center of mass is 


L,=L,cosO,% +LysinO,Yo+L.,% (84) 
The first two terms in the position vector represent the 
location of the shculder. Differentiating the position vector 
gives the express:on for the velocity vector. Because none of 
the coordinate axes used in the position vector expression are 


inertial, their rotation must be included as well. 
£,=L,COSO,Y,- Low .Sin8y% +L,,0,P; (85) 
After Eqs.(83) and (84) are substituded into Eq.(7) and terms 


are grouped by angular rates, the kinetic energy is 


T, =62 (0.5 (I, +m,L2,+m,L;) +m,L,L,, (sin®,sin (6,+8,) +cos (0,+8,) ) ) 
+0. 50? (I, +m,L2;) 
+6,0, (I, +m,L2,+m, LoL, (sin6,sin (6,+8,) +cos8,cos (6,+8,) ) ) 
(86) 


The angular rates for the link between the shoulder and wrist 
(Link 2) includes the centerbody angular rates as well as the 


angular rates of the body axes on Links 1 and 2. 
w,=6,+6, +6, (87) 
This link’s center of mass position vector is 
L,=L,c0s8,%, +L sinO.Jo+L, 2, +Lo292 (88) 


Differentiating the position vector gives the velocity vector 
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£,7L,@,COs8,9, ~L,@,8in8,2, +L, 0,9, +L,,029; (89) 


The kinetic energy for link 2 is found after substituting 
Eqs.(87) and (89) into Eq.(6) and collecting terms with common 


angular rates. 


T,=03 (0.5 (I,+m,Le,+m,L; +m,L¢) 
+m,L,L, (sin6,sin (6,+6,) +cos6,cos (6,+6,) +m,L,L.,cos0, 
+m,L,L,2(sin6@,sin (8, +6, +6,) +cos@,cos (6,+8, +8,) ) ) 
+07 (0.5 (I,+m,L7+m,L2,) +m,L,L,,cos@,) +0. 562 (I, +m,L2,) 
+6,6, (I,+m,Lj +m,L2,+2m,L,L,,cos®, 
+m,L)L, (sin6,sin (6,+6,) +cos6,cos (6,+6,) ) 
+m,L,L,2(sin6,sin (6,+6, +6,) +cos8,cos (6,+6, +8.) ) ) 
+6,0, (I,+m,L3,+m,L,L,,cos0, 
+mL L,2(Ssin8,sin (6,+8, +8,) +cos6,cos (6,+6, +6.) ) ) 
+6,6, (I, +m,Lé2*M,L, 1,,c088,) 
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APPENDIX B: MATLAB CODE 


A. PCONT 


SEVEEESEEEEEEEESESEESEFEEEEEELELESEEEEEEESEEEESEFESEEEEES 
% Main Program to Track a Polynomial Reference Maneuver % 
% pcont.m % 
% calls: ode2.m % 
EESESEEEFEFEEEEEEEEEEEEEEEEEEESEEEEEETELELFESESSESESESEEES 
clg 

clear 


ESESEEFESEELELEES 
% Define States % 
EESEEFEEELEEESEES 


x(1)=thd1 
x (2) =thd2 
x (3) =thd3 
x(4)=thl 
x(5) =th2 
x (6) =th3 


0 dP dP dP OP OP 


SEEEEEEETEEESEEESEEEEEESESES 
% Actual System Constants % 
ES<ELEESEEEEEEEEEEESESEEEESS 


LO = .427; % centerbody r:.dius 

L1 = .530; % length of arm 1 

L2 = .533; % length of arm 2 

LcO = .104; % length to CM of centerbody 
Lcel = .403; % length to CM arm 1 
Le2 = .314; % length to CM arm 2 
mOQ = 65.96; % centerbody mass 
ml = 2.34; % arm 1 mass 

m2 = 2.86; % arm 2 mass 

I0 = 5.74; % centerbody inertia 
I1l = .081; % arm 1 inertia 

I2 = .182; % arm to inertia 


EESEEEEEBELEEEEEEEEEEEEEEEES 
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% Actual System Parameters % 
EEELEEEEELEEEEEEEEELESTEEEEES 


I2 + m2*Lc2%2; 

m2*L1*Lc2; 

m2*LO*Lc2; 

Il + ml*Lco1*2 + m2*L1%2; 
LO*(m1*Lcol + m2*L1); 

IO + mO*Lc0*%2 + LO*2* (ml+m2); 


woud wt wou 


ESELEEEEELESEEESSELEEESEESEEEEEEEEESS 
% Initial Guess of System Constants % 
SESEEEESEFEEEEFEESSEEEESEEESEEESEEEESS 


error = 1; % maximum error for constants 


Lcd0g Lc0+2*error*LcO* (rand-.5); 
Lelg Lcel+2*error*Lcl* (rand-.5); 
Le2g Le2+2*error*Lc2* (rand-.5); 


m0+2*error*m0* (rand-.5); 
ml+2*error*ml* (rand-.5); 
m2+2*error*m2* (rand-.5); 


m0g 
mlg 
m2g 


10g 
Ilg 
I2g 


I0+2*error*I0* (rand-.5); 
I1+2*error*I1* (rand-.5); 
I2+2*error*I2* (rand-.5); 


ESEEEESTEEESESEEEEEEEEEE 
% Initial Conditions % 
EESELEEESEEEEESELELEES 


a0 = zeros(6,1); 


a0(1) 
a0 (2) 
aQ (3) 
a0 (4) 
a0 (5) 
% aN(6 
ad (6) 


I2g + m2g*Lc2g%2; 

m2g*L1*Lc2g; 

m2g*L0*Lc2g; 

Tig + mlg*Lclg*%2 + m2g*Ll1%2; 

LO* (mlg*Lclg + m2g*L1); 

= I0g + m0g*Lc0g*2 + LO*2* (mlg+m2g) ; 
aa(6); 


hm wt Wn on tl 


pO = 100*eye(6); 


tO = 0; 

tfinal = 15 ; 

dt = .01; 

x0 = (0.0; 0.0; 0.0; O*pi/180; -55*pi/180; 15*pi/189]; 





tol = le-6; 
trace = Q; 


SEEEEEEEESESEEEEEEEEESEEES 
% Numerical Integration % 
SESEESETEEEESEEESESSEEESS 


{t,x, thdd,u,a,h,hd,Ref,rxref, ryref ) 


eul (’peq’, tfinal,dt,x0,a0,p0); 


SEXESESEEEESESESESESESEETEESEELETEEETEEEEEEEEELEEES 
% Calculate Momentum Wheel Speed & Tip Position % 
ESEBELEEESSEEEEESEEEEELEESSEESEEEEEETEEEETESEEEEEES 


Iw = 0.0912; % kg-m%*2 

AA = [aa’']; 

thddw = zeros(l,length(t)); 

thdw = zeros(1l,length(t)); 

thdw(1) = 104.7; % rad/sec (= 1000rpm) 
rx zeros(1,length(t)); 


ry zeros(1,length(t)); 

Yr x ( 1 ) 

LO*cos (x(1,4))+L1*cos (sum(x(1,4:5)))+L2*cos (sum(x(1 
x y 1 ) 
LO*sin(x(1,4))+L1*sin(sum(x(1,4:5)))+L2*sin(sum(x(1 


for 1=2:length(t); 
thddw(i) =-u(i,1)/Iw; 
thdw(i) = thddw(i)*(t(i)-t(i-1))+thdw(i-1) ; 
AA=[AA aa’]; 


Xr x ( i 
LO*cos (x(1,4))+L1*cos (sum(x(i,4:5)))+L2*cos (sum(x(1 
xr y ( 2 ) 


LO*sin(x(i,4))+L1*sin(sum(x(i,4:5)))+L2*sin(sum(x(i 
end 


SELEEESEEEEEEEES 
% Plot Output % 
ESEEEEEEEEEEESS 


% figplot 
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,4:6))); 


,4:6))); 


,4:6))); 


14:6))); 


B. PEQ 
SELEEEEEEEESESTESEEEESSEESESEEEESESETEEEEEESEESESEELEEEFEEEEES 
% Equations of Motion for a Polynomial Reference Maneuver % 
% peq.m % 
% called by: ode2.m 

% calls: ref.m, mgm.m, adap.m, angmo.m 
SSETEEECEEEESEELEEEEESESESEESEEEEEEEEEEEEEEEEEEEEESEEEEEEES 


function [xdot,thdd,U,A,P,H,Hd,Ref,rx,ry] = peq(t,x,a,p) 


EEEESELEEESEEKEES 
% Define States % 
SESTEEEELETESESESF 
x(1)=thd0o 
x (2) =thdl 
x(3) =thd2 
x(4)=tho 
x(5)=thl 
x (6) =th2 


EEEEEEEEEESES 
% Constants % 
EESESESESEEES 


LO 
L1 
L2 
L 


.427; % centerbody radius 
.530; % length of arm 1 
.533; % length of arm 2 
[LO0;L1;L2]); 


= 
= 


LcO 
Lel 
Lc2 
Le 


.104; % length to CM of centerbody 
.403; % length to CM arm 1 

314; % length to CM arm 2 
(Lc0;Lcl;Le2]j; 


nou tt 


m0 
m1 
m2 
m 


65.96; % centerbody mass 
2.34; % arm 1 mass 
2.86; % arm 2 mass 

[m0 ;m1;m2]; 


tt toate tt 


10 
Ii 
I2 
ne 


5.74; % centerbody inertia 
.081; % arm 1 inertia 
.182; % arm to inertia 
(I0;11;1I2]; 


ot tt 


ESEEEEEEEEEEEEEEEESEEEFESEES 
% Actual System Parameters % 
EEEEELELEEEEEEEESESEEESEEEES 


aa(l)= I2 + m2*Lc2%2; 





aa(2)= m2*L1*Lc2; 

aa(3)= m2*LO*Lc2; 

aa(4)= Il + mi*Lci*2 + m2*L1%2; 
aa(5)= LO*(m1l*Lcol + m2*Ll); 

aa(6)= I0 + m0*Lc0*%2 + LO*2* (ml+m2); 


SESESESEEESEES 
% Controller % 
SEEEELEEEEELES 


thd(1,1) 
thd(2,1) 
thd(3,1) 


x(1); 
x(2); 
x(3); 


th(1,1) 
th(2,1) 
tht3,.1) 


x(4); 
x(5); 
x(6); 


{(MM,GMj] = mgm(th,thd,a) ; 


Kp=100*eye(3); 
Kv=50*eye (3); 


[Uref,thr,thdr,thddr,rx,ry) = ref(t,L,a); 


Ref=[Uref;thr;thdr;thddr]; 
du=MM* (-Kv* (thd-thdr) -Kp* (th-thr) ); 


U1=MM*thddr+GM; 


U=Uref+du; 
%$ U=Ul+du; 
% U=Uref; 
ESEEEEEEESEES 
% Plant EOM % 
EEEEEEEEEEEES 


{MMa,GMa] = mgm(th,thd, aa); 


thdd inv (MMa) * (U-GMa) ; 
xdot {thdd;x(1);x(2);x(3)]; 


EIEEEEELEFESEEEEELESESESEEESES 
% Adaptive Parameter Update % 
SEETTELEFSEESETEEEEEEEFESEESS 


(A, P,Phi,K) = adap(th,thd,thdd,a,U,p); 
% test=MMa*thdd+GMa-Phi’ *aa’ 





SEEESESEEEEEEESESEEEEES 
% Nonadaptive Control % 
SEREEEEEETEEEEETESEEESES 


tA 
% P 


a; 
Di 


fi 


EESTEEEEEEEETEEEEELESEEEEEESESS 
% Calculate Angular Momentum % 
EEEESEEEEESEEEEEEEEEEEEEEESEES 


(H,Hd] = angmo(m,I,L,Lc,th,thd,thdd) ; 


77 








C. REF 
EEEEEZELELTEESESSFSELESESEEEESETEEESEESEEEEES 
% Function to produce reference parameters % 


% ref.m % 
% called by: peq.m % 
% calls: mgm.m % 


EXSESESEEESESEESESELEEESESSEEEEELEEEEEEEESES 
function [Uref,thr,thdr,thddr,rx,ry] = ref(t,L,a); 


EEEEELESESESEEEEEEEEEESESSELEEEEEEEEEE 
% Initial and Final Angles and Times % 
SEEEEEEEEFESELEEEESEEESSEEEESEEESEEEES 


LO=L(1); 
L1i=L (2); 
L2=L(3); 


0; 
-55*pi/180; 
15*pi/180; 


thori 
thiri 
th2ri 


% always=0 


thorgt 
thir f 
th2rf 


0; % always=0 
40*pi/180; 
15*pi/180; 


ths = 0; % constant 


tO = Q; 
tf = 10; 


SEEEEEEEEEELEELEEEEESESEEEEESEEEEEEEEES 
% Initial And Final Vector Positons % 
EEEEESEEEEEEESSEEEESELESSESESLEESEEEES 


r3x0 = LO*cos(ths)+L1*cos(ths+thlri)+L2*cos(ths+thlri+th2ri); 
r3y0 = LO*sin(ths)+Li*sin(ths+thlri)+L2*sin(ths+thlri+th2ri) ; 
r3xf = LO*cos(ths)+Li*cos(ths+thirf)+L2*cos (ths+thirf+th2rf€); 
r3yf = LO*sin(ths)+Ll1l*sin(ths+thlrf)+L2*sin(ths+thlrf+th2rf) ; 


SEEESSESYEEEEEELESESEESSESESSESSS 
% Calculate Reference Maneuver $% 
EEESSESSEEEESESSESSSELEESESESESES 


tau = (t - t0) / ( tf - tO ); 
({ 10 * tau*3 - 15 * tau*4 + 6 * tau’*5 ); 


( 30 * tau*2 - 60 * tau%*3 + 30 * tau%4); 
(60*tau-180*tau%24+120*tau%3); 


th 
a 
fo at 
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rx 
ry 


rxd = fd*( r3xf - r3x0 )/( tf - tO); 
ryd = fd*( r3yf ~- r3y0 )/( tf - tO ); 


rxdd = £2d* (r3xf~r3x0)/((tf-t0)%2); 
rydd = f2d*(r3yf-r3y0)/((tf£-t0)%2); 


3x0 + ( r3xf - r3x0 ) * 


=f £5 
= r3y0 + ( r3yf - r3y0 ) f; 


if (t>tf); 


rx=r3xf; 
ry=r3yf; 
rxd=0; 
ryd=0; 
rxdd=0; 
rydd=0; 


end 
ESEESESTEESESEESEFESEEEEEESEESEERY 


% Determine Inverse Kinematics % 
SESSEEELEEEESEEESEEEEEEEEEEEESEES 


Sx = LO*cos(ths) ; 

Sy = LO*sin(ths); 

SR = sqrt ((rx-Sx) *2+(ry-Sy)%*2); 

Bl = atan2(ry-Sy,rx-Sx) ; 

B2 = acos((L1°2+SR*2-L2%2) /(2*L1*SR)); 

B3 = acos((L1°2+L2*%2-SR%*2) /(2*L1*L2)); 

thir = Bl-B2-ths; 

th2r = pi-B3; 

thr = [O;thlr;th2r]; 

%$ thr = [thilr;thlr;th2r]; 

H(1,1) = -L2*sin(ths+thlr+th2r)-Li*sin(ths+thlr) ; 
H(1,2) = -L2*sin(ths+thir+th2r) ; 

H(2,1) = L2*cos(ths¢+thlr+th2r)+L1l*cos(ths+thir) ; 
H(2,2) = L2*cos(ths+thlr+th2r) ; 


{thdr12} = inv(H)*[rxd;ryd]; 
thdlr = thdri12(1); 

thd2r = thdr12(2); 

thdr = [0;thdir;thd2r]; 

% thdr = (thdlr;thdlr;thd2r]; 


H da ( ab f 1 = 
~L2* (thdlr+thd2r) *cos (ths+thir+th2r) -Li*thdlr*cos(ths+thlr) ; 
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Hd(1,2) = ~L2* (thdir+thd2r) *cos(ths¢+thir+th2r) ; 

H d ( 2 F 1 ) 

-L2* (thdir+thd2r) *sin(ths+thir+th2r) -L1l*thdlr*sin(ths+thlr) 
Ha(2,2) = ~L2*(thdlr+thd2r) *sin(ths+thlr+th2r); 


[thddr1l2}] = inv(H)*([rxdd;rydd] -Hd* {thdlr;thd2r]); 
thddlr = thddrl2(1); 

thdd2r = thddrl2(2); 

thddr = [0;thddlr;thdd2r]; 


EEEEEEEETELELETESETEETEEESEEEEEEEEELEES 
% Calculate Reference Control Torques % 
SEEEELEEEEEEEEEEFEEEEEEESEEEEELELETEEES 
{(MMr,GMr] = mgm(thr,thdr,a); 


Uref = MMr*thddr+GMr; 


. 
e 





D. EUL 
EELEEEEEELESSEEESEEEEEEEEEEEEEFESEEESESESELEEEEEEEEEEEEEEEESE 
EEEEEEE 
% Discrete Euler Integration 
% 
% eul.m 
% 
% called by:pcont.m 
% 
% calls: peq.m 


% 
FSESEEEEEFLEEEEEEEEEEEEELEEEEEEEEEESESEFEEEFESEEETEEELEEEEELE 
EEEEEEE 
£ u n c t i re) n 

{tout, xout, thddout, uout, aout, Hout, Hdout, Refout, rxout, ryout)= 


eul (FunFen,tf,dt,x0,a0,p0) 


SSSSESEEESESEEESES 
% Initialization % 
SESEEESEEEEEEEEEES 
t = 0; 

xX = x0(:); 

u = zeros(3,1); 

a = a0(:); 

thdd = zeros(3,1); 
Pp = po; 

H = 0; 

Hd = 0; 


Ref=zeros (12,1); 
Ref (4:6) =[(0;-55;15] *pi/180; 
.427; 


tC 
fe 
woud 
16a) 
Ww 
oO 


rxout = [ 
ryout = [ 


SISESEEESESESEEES 
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% The main loop % 
ESEEELEEEELELEEES 


for 1-1: (tfi7dt)+1 
{xd,thdd,u,A,P,H,Hd,Ref,rx,ry] = feval(FunFcn,t,x,a,p)j; 


tout = [tout; t]; 


xout = [xout; x.']; 

thddout = [(thddout; thdd.’‘]; 
uout = [uout; u.'); 

aout = [aout; a.']}; 

Hout = (Hout; HJ; 


Hdout = (Hdout; Hd]; 
Refout = [Refout; Ref.’]); 


rxout = [rxout;rx]; 
ryout = [ryout;ry]; 
t = t + at; 

x = X + xd*dt; 

a= A; 

p= P; 

end 
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EB. MGM 

SSEEEEEEEEEEETEELEEELELELETESEESESEEEEEEEESEEEEETEEST 
% Function to Calculate ‘M’ matrix and ‘G’ vector % 
% mgm.m % 
% called by: peq.m, ref.m % 
SEEEEEEEEEEESEEEEEESEFESFEEEEESESEEEESEEEETETEEEEEEES 


function (MM,GM] = mgm(th,thd,a); 
SSETEEEEEEEEEEELESESEEEEESEEEEEES 


% Define Angles & Angular Rates % 
SSSTTLETEEEEEEEESETEEEEEESESTEEEEEES 


tho = th(1); 
thl = th(2); 
th2 = th(3); 
thd0 = thd(1); 
thdl = thd(2); 
thd2 = thd(3); 


SEEEESSFEESEETELETEEELES 
% Calculate ‘'M’ matrix % 
ESESEESESESESESSSEEEELES 


MM(3,3) = a(1); 

MM(2,3) = MM(3,3)+a(2)*cos(th2); 

MM(3,2) = MM(2,3); 

MM(1,3) = MM(2,3)+a(3)*cos(thl+th2) ; 

MM(3,1) = MM(1,3); 

MM(2,2) = MM(2,3)+a(2)*cos(th2) + a(4); 

MM(1,2) = MM(2,2)+a(3)*cos(thi+th2) + a(5)*cos(thl); 
MM(2,1) = MM(1,2); 

MM(1,1) = MM(2,2)42*a(3)*cos(thl+th2)+2*a(5)*cos(thl)+a(6); 


ESSESESEEESEEEESSESSSEES 
% Calculate ‘G’ vector % 
ESSESESEEESEEZESEEESSEES 


ca2 = thd2*(2*thd0+2*thdl+thd2) *sin(th2) ; 

GM(1,1) = -a(5)*(thdl*2+2*thd0*thd1) *sin(th1) -a(2) *ca2. 
~a(3)*(2*thd0* (thdl+thd2) +(thdl+thd2)*2)*sin(thi+th2) ; 
a(S) ano hee cei) ee C2 GS gen ; 


G M ( 3 ' Hi ) 
a(2)*(thdl+thd2)*2*sin(th2)+a(3)*thd0*2*sin(thl+th2) ; 
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F. ANGMO 
ELETEESELEEEFETESELESETELESESTETEEELETEEEEEEFSEEEEEEE 
% Subroutine to calculate system angular nmomentum % 
% angmo.m 
% called by: peq.m % 
ESESEEEEEEEETESEEEEEESEEEEEEEEEEEESESEEESSEEEEEEEEEES 


function [H,Hd]) =angmo(m,I,L,Lc,th,thd,thdd) ; 
SEEEEEEESEETEEEEEEEESEES 


% Local variable names % 
EXSELEEEEEESEEEEEEEEEEEE 


mO = m(1l); 
ml = m(2); 
m2 = m(3); 
I0 = I{1); 
Il = I(2); 
I2 = I(3); 
LO = L(1); 
L1 = L(2); 
L2 = L(3); 
LcO = Le(l); 
Lcel = Le(2); 
Lce2 = Le(3); 
thoO=th(1); 
thi=eth(2); 
th2=th(3); 
thd0=thd(1); 
thdl=thd(2); 


thd2=thd(3); 


thdd0=thdd(1); 
thddl=thdd (2) ; 
thdd2=thdd(3); 


ESETEESEETEESESEEESELESELESESSS 
% Angular Momentum Equations % 
SSEETESELELESESEESEEESELSSEEESS 


HO 
H1 


thd0* (I10+m0*Lc0%2) ; 

thdO* (I1+m1* (LO0*2+Lc1*%2+2*LO*Lcl*cos(thl)))... 
+thd1* (I1+m1* (Lc1*2+L0*Lcl*cos(thl1))); 
H2 = thd0O* (I2+m2* (LO0*2+L1*%2+Lc2*%2+2*LO*L1*cos(thl).. 
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+2*L1*Lco2*cos (th2)+2*LO*Lc2*cos(thl+th2)))... 
+thdl* (I2+m2* (L1°2+Lc2°2+L0*L1*cos(thl)... 

+2*L1*Lo2*cos (th2)+L0*Lc2*cos(thi+th2)))... 
+thd2* (I2+m2* (Lc2*2+L1*Lc2*cos (th2)+L0*Lco2*cos(thl+th2))); 


H=H0+H1+H2; 


SETEEESEEESEESELS 
% Hdot Equations % 
ELESETESEESEEEEEES 


Hdd thdd0* (10+m0*Lc0%2) ; 

Hdl thddoO* (Il+m1* (LO0*2+Lc1*2+2*LO*Lcl*cos(th1)))... 
+thddl* (I14+ml1* (Lc1%*2+L0*Lcl*cos(th1)))... 
-thd0*thd1*2*m1*L0O*Lcl*sin(thi1)... 
-thd1*2*m1*L0O*Lcl*sin(thl) ; 

Ha2 = thdd0* (I2+m2* (L0%2+L1°%2+Lc2%2+2*LO*L1I*cos(thl)... 

+2*L1*Lo2*cos (th2)+2*LO*Lc2*cos(thl+th2)))... 
+thddl1* (I24+m2* (L1*2+Lc2*2+L0*L1*cos(thl)... 
+2*L1*Lc2*cos (th2)+LO0*Lc2*cos(thl+th2)))... 


+thdd2* (I2+m2* (Lc2°2+L1*Lc2*cos (th2)+L0*Lc2*cos (thi+th2) ) 
-thd0*thdal*2*m2* (LO*L1*sin(th1)+L0*Le2*sin(thl+th2) 
-thd0*thd2*2*m2* (L1*Lc2*sin(th2) +LO*Lc2*sin(thl+th2) 
-thd1*thd2*2*m2* (L1*Lc2*sin(th2)+L0*Lc2*sin(thl+th2) 
-thd1*2*m2* (LO*L1*sin(th1)+L0*Lce2*sin(thl+th2))... 
-thd2*2*m2* (L1*Lc2*sin(th2)+L0*Lc2*sin(thl+th2) ); 


ee 
}s 

) 

) 


Hd=Hd0+Hd1+Hd2; 








G. ADAP 
TELETETTELESESESESEEETELESF 


% Adaption Law % 
% A=adap(th,thd,thdd,a) % 
% called by: peq.m % 


EISEEEEESEEELEEETEEEEEEES 
function [A,P,Phi,K]=adap(th,thd,thdd,a,U,p); 


SEEELELESESEEEEEESELEEEES 
% Local Variable Names % 
SEEEEEEEEEESEETESEEESEEES 


thO=th(1) ; 
thlsth(2); 
th2=th(3); 


thd0=thd(1); 
thdl=thd(2) ; 
thd2=thd (3); 


thdddQ=thdd(1); 
thddl=thdd (2) ; 
thdd2=thdd (3) ; 


ESESSEEEEEEEES 

% Phi Matrix $% 

SESEEEEEEEEEEES 

Phi(1,1) = thdd0+thdd1+thdd2; 

Phi(1,2) = Phi(1,1); 

Phi(1,3) = Phi(1,1); 

Phi(2,1) = (2*thdd0+2*thdd1l+thdd2)*cos(th2)... 


-thd2* (2*thd0+2*thdl+thd2) *sin(th2) ; 


Phi(2,2) = Phi(2,1); 
Phi(2,3) = (thdd0+thddl)*cos(th2) + (thdl+thd2)*2*sin(th2) ; 
Phi(3,1) = (2*thdd0+thdd1l+thdd2)*cos(thl+th2)... 
~(2*thd0* (thdl+thd2) + (thdl+thd2)%*2)*sin(thi+th2); 
Phi(3,2) = thddO*cos(thl+th2) + thd0*2*sin(thl+th2) ; 
Phi (373) = Phit3,;2); 
Phi(4,1) = thdd0+thddl; 
Phi(4,2) = Phi(4,1); 
Phi(4,3) = 0; 
eo ea ao a = (2*thad0+thddl)*cos (tht) = 
(thd1*2+2*thd0*thdl) *sin(thl) ; 
Phi(5,2) = thddO0*cos(thl) + thd0*2*sin(th1); 
Phi(5;,3) =0; 
Phi(6,1) = thdd0; 
Poi(6.2) = 0; 
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Phi(6,3) = 0; 


ELXSESEEEESETEEETEEEEES 
% Adaption Equations % 
EEESEEESEEEEESEEEEEESS 


K = p*Phi*inv(eye(3)+Phi’*p*Phi) ; 


A=a; 

= a+K*(U-Phi’*a); 
P = eye(6); 

= p-K*Phi'‘ *p+eye(6) ; 


~~ 9 » oe 
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APPENDIX C: EXPERIMENTAL CONTROL BLOCK DIAGRAMS 


This appendix includes the diagrams of the system build 


block diagrams made to control the SRS. Both is the parent 


superblock. The others are lower level superblocks. 
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Figure 42: Parameters Block Diagram 
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